/**
* Copyright (c) By zengqh.
*
* This program is just for fun or demo, in the hope that it  
* will be useful, you can redistribute it and/or modify freely.
*
* Time: 2013/02/18
* File: matrix4.cpp
**/

#include "enn_matrix4.h"

namespace enn
{
const Matrix4 Matrix4::ZERO(
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 0.0f);

const Matrix4 Matrix4::IDENTITY(
	1.0f, 0.0f, 0.0f, 0.0f,
	0.0f, 1.0f, 0.0f, 0.0f,
	0.0f, 0.0f, 1.0f, 0.0f,
	0.0f, 0.0f, 0.0f, 1.0f);

void Matrix4::Decompose(vec3f& translation, Quaternion& rotation, vec3f& scale) const
{
	translation.x_ = m03_;
	translation.y_ = m13_;
	translation.z_ = m23_;

	scale.x_ = sqrtf(m00_ * m00_ + m10_ * m10_ + m20_ * m20_);
	scale.y_ = sqrtf(m01_ * m01_ + m11_ * m11_ + m21_ * m21_);
	scale.z_ = sqrtf(m02_ * m02_ + m12_ * m12_ + m22_ * m22_);

	vec3f invScale(1.0f / scale.x_, 1.0f / scale.y_, 1.0f / scale.z_);
	rotation = Quaternion(ToMatrix3().Scaled(invScale));
}

Matrix4 Matrix4::Inverse() const
{
	float v0 = m20_ * m31_ - m21_ * m30_;
	float v1 = m20_ * m32_ - m22_ * m30_;
	float v2 = m20_ * m33_ - m23_ * m30_;
	float v3 = m21_ * m32_ - m22_ * m31_;
	float v4 = m21_ * m33_ - m23_ * m31_;
	float v5 = m22_ * m33_ - m23_ * m32_;

	float i00 = (v5 * m11_ - v4 * m12_ + v3 * m13_);
	float i10 = -(v5 * m10_ - v2 * m12_ + v1 * m13_);
	float i20 = (v4 * m10_ - v2 * m11_ + v0 * m13_);
	float i30 = -(v3 * m10_ - v1 * m11_ + v0 * m12_);

	float invDet = 1.0f / (i00 * m00_ + i10 * m01_ + i20 * m02_ + i30 * m03_);

	i00 *= invDet;
	i10 *= invDet;
	i20 *= invDet;
	i30 *= invDet;

	float i01 = -(v5 * m01_ - v4 * m02_ + v3 * m03_) * invDet;
	float i11 = (v5 * m00_ - v2 * m02_ + v1 * m03_) * invDet;
	float i21 = -(v4 * m00_ - v2 * m01_ + v0 * m03_) * invDet;
	float i31 = (v3 * m00_ - v1 * m01_ + v0 * m02_) * invDet;

	v0 = m10_ * m31_ - m11_ * m30_;
	v1 = m10_ * m32_ - m12_ * m30_;
	v2 = m10_ * m33_ - m13_ * m30_;
	v3 = m11_ * m32_ - m12_ * m31_;
	v4 = m11_ * m33_ - m13_ * m31_;
	v5 = m12_ * m33_ - m13_ * m32_;

	float i02 = (v5 * m01_ - v4 * m02_ + v3 * m03_) * invDet;
	float i12 = -(v5 * m00_ - v2 * m02_ + v1 * m03_) * invDet;
	float i22 = (v4 * m00_ - v2 * m01_ + v0 * m03_) * invDet;
	float i32 = -(v3 * m00_ - v1 * m01_ + v0 * m02_) * invDet;

	v0 = m21_ * m10_ - m20_ * m11_;
	v1 = m22_ * m10_ - m20_ * m12_;
	v2 = m23_ * m10_ - m20_ * m13_;
	v3 = m22_ * m11_ - m21_ * m12_;
	v4 = m23_ * m11_ - m21_ * m13_;
	v5 = m23_ * m12_ - m22_ * m13_;

	float i03 = -(v5 * m01_ - v4 * m02_ + v3 * m03_) * invDet;
	float i13 = (v5 * m00_ - v2 * m02_ + v1 * m03_) * invDet;
	float i23 = -(v4 * m00_ - v2 * m01_ + v0 * m03_) * invDet;
	float i33 = (v3 * m00_ - v1 * m01_ + v0 * m02_) * invDet;

	return Matrix4(
		i00, i01, i02, i03,
		i10, i11, i12, i13,
		i20, i21, i22, i23,
		i30, i31, i32, i33);
}

void Matrix4::makeTransform(const vec3f& position, const vec3f& scale, const Quaternion& ori)
{
	Matrix3 rot3x3;
	rot3x3 = ori.RotationMatrix();

	m00_ = scale.x_ * rot3x3.m00_;
	m01_ = scale.y_ * rot3x3.m01_;
	m02_ = scale.z_ * rot3x3.m02_;
	m03_ = position.x_;

	m10_ = scale.x_ * rot3x3.m10_;
	m11_ = scale.y_ * rot3x3.m11_;
	m12_ = scale.z_ * rot3x3.m12_;
	m13_ = position.y_;

	m20_ = scale.x_ * rot3x3.m20_;
	m21_ = scale.y_ * rot3x3.m21_;
	m22_ = scale.z_ * rot3x3.m22_;
	m23_ = position.z_;

	m30_ = 0;
	m31_ = 0;
	m32_ = 0;
	m33_ = 1;
}

void Matrix4::makeTransform(const vec3f& position, const vec3f& look, const vec3f& up)
{
	vec3f zdir = -look;

	zdir.Normalize();

	vec3f xdir = up.CrossProduct(zdir);

	xdir.Normalize();

	vec3f ydir = zdir.CrossProduct(xdir);

	m00_ = xdir.x_;
	m01_ = ydir.x_;
	m02_ = zdir.x_;
	m03_ = position.x_;
	m10_ = xdir.y_;
	m11_ = ydir.y_;
	m12_ = zdir.y_;
	m13_ = position.y_;
	m20_ = xdir.z_;
	m21_ = ydir.z_;
	m22_ = zdir.z_;
	m23_ = position.z_;
	m30_ = 0.0f;
	m31_ = 0.0f;
	m32_ = 0.0f;
	m33_ = 1.0f;
}

}